﻿/// 单点标定方法: 选取4个位置， tcp尖点坐标不动，可用下面算法标定
/// 具体参见  https://blog.csdn.net/qq_21834027/article/details/85127629

#include <iostream>

#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <opencv2/core.hpp>

#include <memath/matrixd.h>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace std;
using namespace cv;

/// 4个指向目标点 pIn 的旋转矩阵和平移矩阵
static void simu4Pose(Eigen::Vector3d &pIn, vector<Eigen::Matrix3d> &R, vector<Eigen::Vector3d> &t)
{
    R.clear();
    t.clear();

    Eigen::Vector3d &p = pIn;

    Eigen::AngleAxisd R1 = Eigen::AngleAxisd(M_PI * 0/180, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd R2 = Eigen::AngleAxisd(M_PI * 90/180, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd R3 = Eigen::AngleAxisd(-M_PI * 90/180, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd R4 = Eigen::AngleAxisd(M_PI * 90/180, Eigen::Vector3d::UnitY());

    R.push_back(R1.toRotationMatrix());
    R.push_back(R2.toRotationMatrix());
    R.push_back(R3.toRotationMatrix());
    R.push_back(R4.toRotationMatrix());

    t.push_back(p - R1*p);
    t.push_back(p - R2*p);
    t.push_back(p - R3*p);
    t.push_back(p - R4*p);

}

/// 计算围绕旋转的一个点，也就是机器人标定tcp位置的算法
static Eigen::Vector3d calibPoint(vector<Eigen::Matrix3d> &R, vector<Eigen::Vector3d> &t)
{
    // $ p_{bt} = R_{be} * p_{et} + t_{be} $

    Eigen::MatrixXd A(9, 3);
    Eigen::VectorXd b(9);
    uint i = 0,j,k;
    for (i = 0; i< R.size()-1; i++) {
        for (j = 0; j < 3; j++) {
            for(k = 0; k<3; k++) {
                A(i*3+j, k) = R[i](j,k) - R[i+1](j,k);

            }
            b[i*3+j] = t[i+1][j] - t[i][j];
        }
    }

    //cout << "A:\n" << A << endl;
    //cout << "b:\n" << b << endl;

    //Eigen::Vector3d x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    Eigen::Vector3d x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);


    // cout << "result: \n" << x << endl;
    return x;
}

/// 通过模拟数据验证算法是否OK
static void demoCalPointWithSimuData()
{
    // $ p_{bt} = R_{be} * p_{et} + t_{be} $
    // 1. 创建/模拟 >=4个是个旋转矩阵 p_be[4] R_be[4]
    Eigen::Vector3d p_et(0, 0, 10);
    vector<Eigen::Matrix3d> R;
    vector<Eigen::Vector3d> t;
    simu4Pose(p_et, R, t);
    for (uint i = 0; i < R.size(); i++) {
        cout << i+1 << " R: " << R[i] << "\n t: " << t[i].transpose() << endl;
    }

    // 2. 列出Ax=b方程
    // t[0][0] = 0.5;  // 加扰动
    Eigen::Vector3d pp_et = calibPoint(R, t);

    // 3. 输出解

    cout << "result: \n" << pp_et.transpose() << " expected:" << p_et.transpose() << endl;
}

static void help()
{
    static const char * usage =
            "Usage: zDemoCalibPoint <option>\n"
            "option: \n"
            "   1 demoCalPointWithSimuData \n"
            ;
}

int main(int argc, char *argv[])
{
    int option =1;

    if (argc == 2) {
        option = atoi(argv[1]);
    }

    switch (option) {
    case 1:
        demoCalPointWithSimuData();
        break;
    default:
        break;
    }
    return 0;
}
